/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class RobotMain extends IterativeRobot {

    /**
     * allows for easy changing of channels
     */
    //ENCODER SLOTS!
    public static final int leftEncoderSlot1 = 1; 
    public static final int leftEncoderSlot2 = 2; 
    public static final int rightEncoderSlot1 = 3;
    public static final int rightEncoderSlot2 = 4;
    public static final int turretEncoderSlot1 = 5;
    public static final int turretEncoderSlot2 = 6;
    // vitors slots
    public static final int turretRotationVictorSlot = 5;
    public static final int shooterVictorSlot = 2;
    public static final int gatherVictorSlot = 3;
    public static final int storageVictorSlot = 4; 
    public static final int bridgeVictorSlot = 1;
   // jag slots
    public static final int leftTrainJagSlot = 9; 
    public static final int rightTrainJagSlot = 8;
    // variables and constants relating to the PID and Encoder controls
    private static final double rightEncoderDistancePerPulse = (((1/2.133) * ((2 * Math.PI)/360))*3);
    private static final double leftEncoderDistancePerPulse = (((1/2.133) * ((2 * Math.PI)/360))*3);
    private static final double kPIDPeriod = 0.050;
    private static final double kEncoderPeriod = kPIDPeriod - 0.005;
    private static final double maxEncoderReturnRate = 20;
    private static final double minEncoderReturnRate = -20;
    // variables relating to gear shifting
    private static final double lowGearPosition = 1.0;
    private static final double highGearPosition = 0.0;
    // variables relating to the drive train
    private double curve = .5;
    private double deadband = .05;
    // variables relating to end effectors
    private static final double gatherSpeed = .5;
    public boolean highGear;
    private double bridgeArmSpeed = 0.5;
    //DECLARING OBJECTS
    //encoders
    private Encoder rightEncoder = new RateEncoder(rightEncoderSlot1, rightEncoderSlot2,
            minEncoderReturnRate, maxEncoderReturnRate,
            kPIDPeriod);
    private Encoder leftEncoder = new RateEncoder(leftEncoderSlot1, leftEncoderSlot2,
            minEncoderReturnRate, maxEncoderReturnRate,
            kPIDPeriod);
    private Encoder turretEncoder = new Encoder(turretEncoderSlot1, turretEncoderSlot2);
    //victors
    private Victor shooterVictor = new Victor(shooterVictorSlot);
    private Victor gatherVictor = new Victor(gatherVictorSlot);
    private Victor storageVictor = new Victor(storageVictorSlot);
    private Victor bridgeVictor = new Victor(bridgeVictorSlot);
    private Victor turretRotationVictor = new Victor(turretRotationVictorSlot);
    //  private Victor myVictor = new Victor(myVictorSlot); //TODO find out what victor will do
    //jaguars
    private Jaguar leftTrainJag = new Jaguar(leftTrainJagSlot);
    private Jaguar rightTrainJag = new Jaguar(rightTrainJagSlot);
    //servos
    private GearsServo gearsServo = new GearsServo(10, highGearPosition, lowGearPosition);

    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        System.out.println("build complete");
        gearsServo.setHighGear();
        leftEncoder.setDistancePerPulse(leftEncoderDistancePerPulse);
        rightEncoder.setDistancePerPulse(rightEncoderDistancePerPulse);

    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        update();
    }

    public void update() {
        updateShifter();
        updateDrive();
        updateGatherer();
        updateSmartDashboard();
        updateBridgeManipulator();
        //updateShooter
        //updatesStorage
        //updateBalance
        //updatePID
    }
// NOTE TO SELF - CHANGE SO THAT currentgear = fast IN ROBOTINTIT (CANNOT START NOT IT A GEAR!!!!)
// completed this 2/5/2012

    // TODO make shifter a toggle
    public void updateShifter() {
          if (Controller.getInstance().Shift(highGear)) {
            gearsServo.setHighGear();
        }
        if (Controller.getInstance().Shift(highGear)) {
            gearsServo.setLowGear();
        }
    }


    //updates the bridge manipulator
    public void updateBridgeManipulator() {
        if (Controller.getInstance().lowerBridgeArmButton()) {

            bridgeVictor.set(bridgeArmSpeed);

        }
        if (Controller.getInstance().raiseBridgeArmButton()) {

            bridgeVictor.set(-bridgeArmSpeed);

        }
        if (!Controller.getInstance().raiseBridgeArmButton()
                && !Controller.getInstance().lowerBridgeArmButton()) {

            bridgeVictor.set(0);

        }
    }

    //updates the current drive speed
    public void updateDrive() {
        curve = Controller.getInstance().getCurve();
        double rightSpeed = Controller.getInstance().GetRightSpeed(curve, deadband);
        double leftSpeed = Controller.getInstance().GetLeftSpeed(curve, deadband);
        rightTrainJag.set(-1 * rightSpeed);
        leftTrainJag.set(leftSpeed);
    }
//updates the current speed of the bridge manipulator

    public void updateGatherer() {
        if (Controller.getInstance().gathererOnbutton()) {
            gatherVictor.set(gatherSpeed);

        }
        if (Controller.getInstance().gathererOffButton()) {
            gatherVictor.set(0);
        }


    }

    //updates the smartdashboard
    public void updateSmartDashboard() {
        SmartDashboard.putDouble("curve", curve);
        SmartDashboard.putDouble("deadband", deadband);
        SmartDashboard.putBoolean("highgear", highGear);
        SmartDashboard.putDouble("left joystick", Controller.getInstance().GetLeftSpeed(curve, deadband));
        SmartDashboard.putDouble("right joystick", Controller.getInstance().GetRightSpeed(curve, deadband));
        SmartDashboard.putBoolean("PID enabled", Controller.getInstance().getPIDEnabled());
        SmartDashboard.putDouble("left encoder count", leftEncoder.get());
        SmartDashboard.putDouble("right encoder count", rightEncoder.get());
        SmartDashboard.putDouble("left distance", leftEncoder.getDistance());
       SmartDashboard.putDouble("right distance", rightEncoder.getDistance());
       SmartDashboard.putString("powerlevel","over 9000");
        //Shooter speed
        //turret position
        //gatherer on/off

    }
}
